#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
//因为小海龟在接收的时候用的是geometry_msgs::msg::Twist这个类型，因为要包含这个类型的头文件
#include <chrono>
 //引入时间相关头文件

//使用时间单位的字面量，可以在代码中使用 s 和 ms 表示时间
using namespace std::chrono_literals;

class TurtleMove : public rclcpp::Node
{
private:
  rclcpp::TimerBase::SharedPtr timer_; //定时器的智能指针
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_; //发布者的智能指针
  int state_ =0; //定义一个变量，用来表示当前这一时刻是在前进还是再后退
  int times_ =0; //用来记录已经前进了多长时间

public:
  //explicit 关键字用来防止在编译的时候发生隐式类型转化
  explicit TurtleMove(const std::string& node_name) : Node(node_name)
  {
    //调用继承而来的父类函数创建订阅者
    publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel", 10);
    //调用继承而来的父类函数创建定时器
    timer_ = this->create_wall_timer(1000ms, std::bind(&TurtleMove::timer_callback, this));
  }

private:
  void timer_callback()
  {
    auto msg = geometry_msgs::msg::Twist();
    //实例化一个类型对象，主要包含linear，angular，表示线速度和角速度
    if (state_==0) {//表示当前处于将要前进或正在前进的状态
      msg.linear.x = 1.0; //让它前进
      msg.angular.z = 0.0;
      times_++;
      if (times_ >= 2) { //当前进2秒后切换到转弯状态
        state_ = 1;
        times_ = 0;//将前进时间回复，方便下一次在进行计数
      }
    } else {//当前处于转弯状态
      msg.linear.x = 0.0;
      msg.angular.z = 0.5 * M_PI_2; //M_PI_2是π/2,这句话也就是将它的角速度变成π/4
      times_++;
      if (times_ >= 2) { //因为角速度是π/4，所以旋转π/2需要2秒
        state_ = 0;//切换回前进状态
        times_ = 0;//将前进时间回复，方便下一次在进行计数
      }
    }
    publisher_->publish(msg);//发布
  }

};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<TurtleMove>("turtle_square");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}